'''
1、接受tessng向kafka发送的车辆数据,并进行展示

2、单个相机和雷达,并将相机的图片和雷达的点云进行保存

'''

import carla
import numpy as np
import time
from carla import Transform, Rotation, Location
from kafka import KafkaConsumer
import json
from pyproj import Proj
import numpy as np
import cv2
import os
import copy



IM_WIDTH = 640
IM_HEIGHT = 480

# 经纬度转xy坐标，在carla中y坐标的值需要取反
rule = Proj("+proj=tmerc +lat_0=40.04752343063863 +lon_0=116.2872228823074 +k=1 +x_0=0 +y_0=0 +datum=WGS84")

def get_time_stamp(ct):

    local_time = time.localtime(ct)
    data_head = time.strftime("%Y-%m-%d %H-%M-%S", local_time).split(' ')[-1]
    data_secs = (ct - int(ct)) * 1000
    time_stamp = "%s-%03d" % (data_head, data_secs)
    return time_stamp

# 将雷达的点云保存到本地 
def save_pcd(lidar_data,world,lidar_name):
    points = np.frombuffer(lidar_data.raw_data, dtype=np.dtype('f4'))
    points = copy.deepcopy(points)
    points = np.reshape(points, (int(points.shape[0] / 4), 4))
    
    # 存放路径

    p_timestamp = world.get_snapshot().timestamp.platform_timestamp
    w_timestamp = get_time_stamp(p_timestamp)
    path = save_path + "pcd/" + "_" + lidar_name + "_" + str(w_timestamp) + ".pcd"
    
    if os.path.exists(path):
        os.remove(path)

    # 写文件句柄
    handle = open(path, 'a')

    # 得到点云点数
    point_num = points.shape[0]

    # pcd头部（重要）
    handle.write(
        '# .PCD v0.7 - Point Cloud Data file format\nVERSION 0.7\nFIELDS x y z intensity\nSIZE 4 4 4 4\nTYPE F F F F\nCOUNT 1 1 1 1')
    string = '\nWIDTH ' + str(point_num)
    handle.write(string)
    handle.write('\nHEIGHT 1\nVIEWPOINT 0 0 0 1 0 0 0')
    string = '\nPOINTS ' + str(point_num)
    handle.write(string)
    handle.write('\nDATA ascii')

    # 依次写入点
    for i in range(point_num):
        string = '\n' + str(points[i, 0]) + ' ' + str(points[i, 1]) + ' ' + str(points[i, 2]) + ' ' + str(points[i, 3])
        handle.write(string)
    handle.close()

# 将相机图片保存到本地
def save_camera_image(image,world,camme_name):
    p_timestamp = world.get_snapshot().timestamp.platform_timestamp
    w_timestamp = get_time_stamp(p_timestamp)
    path = save_path + "png/" + "_" + camme_name + "_" + str(w_timestamp) + ".png"
    # 将Carla图像转换为OpenCV图像格式（BGR）
    image_data = image.raw_data
    image_array = np.array(image_data)
    image_bgra = image_array.reshape((image.height, image.width, 4))
    image_rgb = cv2.cvtColor(image_bgra, cv2.COLOR_BGRA2RGB)
    # 保存图像
    cv2.imwrite(path, image_rgb)


def main():
  
    try:
                     
    #  ======================================跟服务器实现连接=============================================
        client = carla.Client('10.100.16.98', 2000)
        client.set_timeout(10.0)
                 
    #  ======================================使用自定义地图=============================================
        world = client.load_world('tongji')

    # =============================天气=================================

        # 控制世界的天气和时间（太阳的位置） 万里无云，没有降雨，太阳的角度为90
        weather = carla.WeatherParameters(
            cloudiness=0.0,  # 0-100  0 是晴朗的天空，100 是完全阴天
            precipitation=0.0,  # 0 表示没有下雨，100 表示大雨
            precipitation_deposits=0.0, # 0 表示道路上没有水坑，100 表示道路完全被雨水覆盖
            wind_intensity=0.0, # 0 表示平静，100 表示强风，风会影响雨向和树叶
            sun_azimuth_angle=0.0, # 太阳方位角，0～360
            sun_altitude_angle=90.0,  # 太阳高度角，90 是中午，-90 是午夜
            fog_density=0.0, # 0～100 表示雾的浓度或厚度，仅影响RGB相机传感器
            fog_distance=0.0, # 雾开始的距离，单位为米
            wetness=0.0, # 0～100 表示道路湿度百分比，仅影响RGB相机传感器
            fog_falloff=0.0, # 雾的密度，0至无穷大，0 表示雾比空气轻，覆盖整个场景，1表示与空气一样，覆盖正常大小的建筑物
            scattering_intensity=0.0, # 控制光线对雾的穿透程度
            mie_scattering_scale=0.0, # 控制光线与花粉或空气等大颗粒的相互作用，导致天气朦胧，光源周围有光晕，0表示无影响
            rayleigh_scattering_scale=0.0331, # 控制光与空气分子等小粒子的相互作用，取决于光波长，导致白天蓝天或晚上红天
            )
        world.set_weather(weather)
        
        
        
        # =============================创建kafka消费者=================================
        topic = 'tess_send_carla_shangdi'
        bootstrap_servers = '106.120.201.126:14576'
        # 创建Kafka消费者实例
        consumer = KafkaConsumer(
            topic,
            bootstrap_servers = bootstrap_servers,
            value_deserializer=lambda x: json.loads(x.decode('utf-8'))  # 指定反序列化函数为JSON解析
        )    
        
        # =============================创建交通管理器对象，并设置仿真的倍数和车辆最大的速度=================================  
    
        traffic_manager = client.get_trafficmanager(8000)
        # traffic_manager.set_global_port(8100)
        traffic_manager.set_global_distance_to_leading_vehicle(2.0)
        traffic_manager.global_percentage_speed_difference(40.0)
        
        if True:
            settings = world.get_settings()
            traffic_manager.set_synchronous_mode(True)
            if not settings.synchronous_mode:
                print("开启异步模式")
                synchronous_master = True
                settings.synchronous_mode = True
                settings.fixed_delta_seconds = 0.1
                world.apply_settings(settings)
            else:
                synchronous_master = False
        
    # =============================视角坐标=================================
        x, y = rule(116.29220138718945, 40.05073628413568)



        vehicle_trans11 = Transform(Location(x=424.81892146460007, y=-356.75291758771124, z=65.677723), 
                                    Rotation(yaw = 270))
        world.get_spectator().set_transform(vehicle_trans11)  # 设置世界视角
        
        # =============================设置车辆=================================
        
        vehicle_trans12 = Transform(Location(x=x, y=-y, z=41.677723), 
                                    Rotation(yaw = 347.65-90))
        

        blueprint_library = world.get_blueprint_library()
        bp1 = blueprint_library.find('vehicle.lincoln.mkz_2017')
        bp1.set_attribute('color', '0, 0, 0')
        ego_vehicle1 = world.spawn_actor(bp1, vehicle_trans12)
        actor_list.append(ego_vehicle1)
        
    # =======================================相机==================================
    
    
        vehicle_trans_rgb = Transform(Location(x=x, y=-y, z=61.677723), 
                                    Rotation(yaw = 347.65-90))
    
        cam_bp = blueprint_library.find('sensor.camera.rgb')  
        # 设置传感器采集图片的尺寸
        cam_bp.set_attribute("image_size_x", f'{600}')
        cam_bp.set_attribute("image_size_y", f'{500}')
        # 设置摄像头的水平翻转
        cam_bp.set_attribute("fov", "90")
        # 设置传感器捕获之间的时间（以秒为单位）
        # cam_bp.set_attribute('sensor_tick', '1')
        # 
        cam01 = world.spawn_actor(cam_bp, vehicle_trans_rgb,attach_to=None)
     
        
        cam01.listen(lambda data: save_camera_image(data,world,"cam01"))
        actor_list.append(cam01)
   # =======================================雷达==================================
        vehicle_trans_lidar = Transform(Location(x=x, y=-y+3, z=41.677723), 
                                    Rotation(yaw = 347.65-90))
     
        lidar_bp_16 = blueprint_library.find('sensor.lidar.ray_cast')  
        
        lidar_bp_16.set_attribute('channels', '16')# 扫描点的通道数量
        lidar_bp_16.set_attribute('upper_fov', '-10')#垂直方向上的视野范围（弧度表示，0到佩）
        lidar_bp_16.set_attribute('lower_fov', '-30')# 
        lidar_bp_16.set_attribute('dropoff_general_rate', '0.0')# 
        lidar_bp_16.set_attribute('points_per_second', '288000')# 每秒中生成的扫描点数
        lidar_bp_16.set_attribute('range', '200')# 最大探测距离（米）
        lidar_bp_16.set_attribute('rotation_frequency', str(int(1 / settings.fixed_delta_seconds)))# 旋转的频率
        
        lidar_16 = world.spawn_actor(lidar_bp_16, vehicle_trans_lidar, attach_to=None)
        
        # lidar_16.listen(lambda data: save_pcd(data, world, "lidar_16"))
        
        sensor_list.append(lidar_16)
               
        # 持续消费消息
        while True:
        
            #车辆信息
            for message in consumer:
                # 解析Kafka消息中的车辆位置信息
                json_string = message.value
                participants = json_string["participants"]
                errorids = []
                nowids = []
                for p in participants:
                    nowids.append(p["ID"])
                #添加交通流
                for par in participants:

                    x, y = rule(par["longitude"], par["latitude"])
                    
                    trans = Transform(carla.Location(x=x, y=-y, z=37),
                                        carla.Rotation(yaw= par["courseAngle"]-90))
                    
                    print(par["courseAngle"])
                    vehtype = par['type']
                    vehtype = 'vehicle.lincoln.mkz_2017'
                    id = par['ID']
                    
                    if id not in spawed_ids.keys():
                        bp1 = world.get_blueprint_library().find(vehtype)
                        try:
                            bp1.set_attribute('color', '0,0,0')
                        except:
                            pass
                        else:
                            pass

                        if vehtype == 'vehicle.lincoln.mkz_2017':
                            batch = [
                                carla.command.SpawnActor(bp1, trans).then(
                                    carla.command.SetSimulatePhysics(carla.command.FutureActor, False))
                            ]
                            response = client.apply_batch_sync(batch, False)[0]
                            if response.error:
                                errorids.append(id)
                            else:
                                spawed_ids[id] = response.actor_id
                        else:
                            batch = [
                                carla.command.SpawnActor(bp1, trans).then(
                                    carla.command.SetSimulatePhysics(carla.command.FutureActor, False))
                            ]
                            response = client.apply_batch_sync(batch, False)[0]
                            if response.error:
                                errorids.append(id)
                            else:
                                spawed_ids[id] = response.actor_id
        
                    else:
                        if vehtype == 'vehicle.lincoln.mkz_2017':
                            cid = spawed_ids[id]
                            cactor = world.get_actor(cid)
                            cactor.set_transform(trans)
                        else:
                            carlaid = spawed_ids[id]
                            vehicle = world.get_actor(carlaid)
                            if vehicle is not None:
                                vehicle.set_transform(trans)
                print("已有：", len(spawed_ids))

                # 删除车辆
                destodyed_ids = [id for id in spawed_ids if id not in nowids]
                for did in destodyed_ids:
                    carlaid = spawed_ids[did]
                    vehicle = world.get_actor(carlaid)

                    if vehicle is not None:
                        vehicle.destroy()
                    del spawed_ids[did]
                print("更新：", len(nowids), "销毁:", len(destodyed_ids))

                
          

    finally:


        if synchronous_master:
            settings = world.get_settings()
            settings.synchronous_mode = False
            settings.fixed_delta_seconds = None
            world.apply_settings(settings)
        else:
            settings = world.get_settings()
            settings.synchronous_mode = False
            settings.fixed_delta_seconds = None
            world.apply_settings(settings)

        cv2.destroyAllWindows()
        for sensor in sensor_list:
            sensor.destroy()
        for actor in actor_list:
            if actor.is_alive:
                actor.destroy()
                

                
                    
     

if __name__ == "__main__":
    

    spawed_ids = {}
    actor_list, sensor_list = [], []
    save_path = r'D:\hsy\12-预控-carla\tongji-carla\carla\tongji\data'

    
    try:
        main()
    except KeyboardInterrupt:
        print(' - Exited by user.')
